/**
 * M5麦克纳姆轮遥控小车M5麦克纳姆轮遥控小车
 * 
 * https://www.xpstem.com/maker/m5-mecanumcar
 * author: billy zhang
 */

#include "M5Stack.h"
#include "LegoMotor.h"
#include <WiFi.h>
#include <WiFiMulti.h>
#include <WebServer.h>
#include "Constants.h"

#define BASE_X_ADDR (0x22)

#define NORMAL_MODE     (0x00)
#define POSITION_MODE   (0x01)
#define SPEED_MODE      (0x02)

WiFiMulti WiFiMulti;

WebServer server(80);

// 马达端口视连续情况而定
LegoMotor motorTL = LegoMotor(BASE_X_ADDR, 3); //马达对象，3为对应连接端口
LegoMotor motorTR = LegoMotor(BASE_X_ADDR, 0);
LegoMotor motorBL = LegoMotor(BASE_X_ADDR, 2);
LegoMotor motorBR = LegoMotor(BASE_X_ADDR, 1);

String strs = "abcdefghijkmnpqrstuvwxyz0123456789";
String clientId = "";  // 客户端ID

/* 
 * 生成指定位数的随机字符串
 */
String generateRandomStr(int len) 
{
  String rndStr = "";
  int strLen = strs.length();
  
  for (int i=0; i<len; i++) 
  {
     rndStr.concat(strs.charAt(random(0, strLen-1)));
  }
  return rndStr;
}

void setup()
{
    M5.begin(true, false, true, true);
    M5.Power.begin();

    motorTL.SetMode(NORMAL_MODE);
    motorTR.SetMode(NORMAL_MODE);
    motorBL.SetMode(NORMAL_MODE);
    motorBR.SetMode(NORMAL_MODE);

    int sum=0;
    WiFiMulti.addAP(ssid, password);  // 添加wifi配置信息
    M5.lcd.printf("Waiting connect to WiFi: %s ...",ssid); // 输出格式化字符串
    while(WiFiMulti.run() != WL_CONNECTED) {  //  如果没有与wifi成功建立连接
        M5.lcd.print(".");
        delay(1000);
        sum+=1;
        if(sum==8) M5.lcd.print("Conncet failed!");
    }
    M5.lcd.println("\nWiFi connected");
    M5.lcd.print("IP address: ");
    M5.lcd.println(WiFi.localIP()); // 输出M5Core的IP地址

    // 定义控制处理
    server.on("/control", control);

    // 定义连接处理
    server.on("/connect", []() {
        clientId = generateRandomStr(16); // 手机连接后，返回一个随机字符串做为客户端ID
        server.send(200, "text/plain", clientId);
    });
   
    server.begin();
    M5.lcd.println("HTTP server started");
}       

void loop()
{
    server.handleClient();
    delay(2);
}

void control()
{
    String cid = server.arg("cid"); //客户端ID
    String cmd = server.arg("cmd"); //操作命令
    uint8_t power = server.arg("power").toInt(); //功率
    if (clientId.compareTo(cid) != 0)
    {
      //客户端ID不一致，不允许操作。
      server.send(403);
    }
   
    M5.lcd.printf("handle cmd: %s, power: %d\n", cmd, power);
    
    uint8_t n = 1;

    // 小车操作指令使用字符串，也可以使用数字。
    // 停止
    if (String("stop").compareTo(cmd) == 0) 
    {
        stop();
        server.send(200);
        return;
    } 

    // 前进
    if (String("forward").compareTo(cmd) == 0) 
    {
        stop();
        forward(power);
        server.send(200);
        return;
    } 

    // 后退
    if (String("backward").compareTo(cmd) == 0) 
    {
        stop();
        backward(power);
        server.send(200);
        return;
    }

    // 向左横移
    if (String("moveleft").compareTo(cmd) == 0) 
    {
        stop();
        moveLeft(power);
        server.send(200);
        return;
    }

    // 向右横移
    if (String("moveright").compareTo(cmd) == 0) 
    {
        stop();
        moveRight(power);
        server.send(200);
        return;
    }

    // 未匹配到指令
    server.send(400);
    
}

void stop() 
{
    motorTL.SetMotorSpeed(0);
    motorTR.SetMotorSpeed(0);
    motorBL.SetMotorSpeed(0);
    motorBR.SetMotorSpeed(0);
    delay(50); //等待马达停下来
}

//视马达和轮子的齿轮连接做正/反转处理，负功率为反转。

void forward(uint8_t power)
{
    motorTL.SetMotorSpeed(-power); //reversal
    motorTR.SetMotorSpeed(power);
    motorBL.SetMotorSpeed(power);
    motorBR.SetMotorSpeed(-power); //reversal
}

void backward(uint8_t power)
{
    motorTL.SetMotorSpeed(power);
    motorTR.SetMotorSpeed(-power); //reversal
    motorBL.SetMotorSpeed(-power); //reversal
    motorBR.SetMotorSpeed(power);
}

void moveLeft(uint8_t power)
{
    motorTL.SetMotorSpeed(power);
    motorTR.SetMotorSpeed(power); 
    motorBL.SetMotorSpeed(power); 
    motorBR.SetMotorSpeed(power);  
}

void moveRight(uint8_t power)
{
    motorTL.SetMotorSpeed(-power); //reversal
    motorTR.SetMotorSpeed(-power); //reversal
    motorBL.SetMotorSpeed(-power); //reversal
    motorBR.SetMotorSpeed(-power); //reversal
}
